/*!
* @file Rotation.cpp
* @brief 三维旋转
* @date 2024/9/11
*/

#include "Rotation.h"
#include "Matrix4D.h"

using namespace data;

Rotation::Rotation(const Vector3d& axis, double angle)
{
    Set(axis, angle);
}

Rotation::Rotation(const Vector3d& from, const Vector3d& to)
{
    Set(from, to);
}

void Rotation::Get(double& fx, double& fy, double& fz, double& fw) const
{
    fx = x;
    fy = y;
    fz = z;
    fw = w;
}

void Rotation::Set(double fx, double fy, double fz, double fw)
{
    x = fx;
    y = fy;
    z = fz;
    w = fw;
}

void Rotation::Set(const double q[4])
{
    x = q[0];
    y = q[1];
    z = q[2];
    w = q[3];
}

void Rotation::Get(Vector3d& axis, double& angle) const
{
    // Note: -1 < w < +1 (|w| == 1 not allowed, with w:=quat[3])
    if ((w > -1.0) && (w < 1.0)) {
        angle = acos(w) * 2.0;
        double scale = sin(angle / 2.0);
        // Get a normalized vector
        axis.x = x / scale;
        axis.y = y / scale;
        axis.z = z / scale;
    }
    else {
        axis.Set(0.0, 0.0, 1.0);
        angle = 0.0;
    }
}

void Rotation::Set(const Vector3d& axis, double angle)
{
    double len = axis.Length();
    if (len < traits_type::epsilon())
    {
        // ~zero length axis, so reset rotation to zero.
        *this = {};
        return;
    }

    double inversenorm = 1.0 / len;
    double coshalfangle = cos(0.5 * angle);
    double sinhalfangle = sin(0.5 * angle);

    x = axis.x * sinhalfangle * inversenorm;
    y = axis.y * sinhalfangle * inversenorm;
    z = axis.z * sinhalfangle * inversenorm;
    w = coshalfangle;
}

void Rotation::Set(const Vector3d& from, const Vector3d& to)
{
    Vector3d u(from);
    u.Normalize();
    Vector3d v(to);
    v.Normalize();

    // The vector from x to is the rotation axis because it's the normal of the plane defined by
    // (0,u,v)
    double dot = u * v;
    Vector3d w = u % v;
    double wlen = w.Length();

    if (wlen < traits_type::epsilon()) {  // Parallel vectors
        // Check if they are pointing in the same direction.
        if (dot > 0.0) {
            Set(0.0, 0.0, 0.0, 1.0);
        }
        else {
            // We can use any axis perpendicular to u (and v)
            Vector3d t = u % Vector3d(1.0, 0.0, 0.0);
            if (t.Length() < traits_type::epsilon()) {
                t = u % Vector3d(0.0, 1.0, 0.0);
            }
            Set(t.x, t.y, t.z, 0.0);
        }
    }
    else {  // Vectors are not parallel
        // Note: A quaternion is not well-defined by specifying a point and its transformed point.
        // Every quaternion with a rotation axis having the same angle to the vectors of both points
        // is okay.
        double angle = acos(dot);
        Set(w, angle);
    }
}

Rotation& Rotation::operator*=(const Rotation& q)
{
    return MultRight(q);
}

Rotation Rotation::operator*(const Rotation& q) const
{
    Rotation quat(*this);
    quat *= q;
    return quat;
}

bool Rotation::operator==(const Rotation& q) const
{
    return ((x == q.x && y == q.y && z == q.z && w == q.w) ||
        (x == -q.x && y == -q.y && z == -q.z && w == -q.w));
}

bool Rotation::operator!=(const Rotation& q) const
{
    return !(*this == q);
}

double& Rotation::operator[](unsigned short index)
{
    switch (index) {
    case 0:
        return x;
    case 1:
        return y;
    case 2:
        return z;
    case 3:
        return w;
    }
    return x;
}
const double& Rotation::operator[](unsigned short index) const
{
    switch (index) {
    case 0:
        return x;
    case 1:
        return y;
    case 2:
        return z;
    case 3:
        return w;
    }
    return x;
}

Rotation& Rotation::MultRight(const Rotation& q)
{
    double x0, y0, z0, w0;
    Get(x0, y0, z0, w0);

    double x1, y1, z1, w1;
    q.Get(x1, y1, z1, w1);

    Set(w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1,
        w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1,
        w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1,
        w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1);
    return *this;
}

Rotation& Rotation::MultLeft(const Rotation& q)
{
    double x0, y0, z0, w0;
    q.Get(x0, y0, z0, w0);

    double x1, y1, z1, w1;
    Get(x1, y1, z1, w1);

    Set(w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1,
        w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1,
        w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1,
        w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1);
    return *this;
}

Vector3d Rotation::MultVec(const Vector3d& src) const
{
    Vector3d dst;
    double x2 = x * x;
    double y2 = y * y;
    double z2 = z * z;
    double w2 = w * w;

    double dx =
        (x2 + w2 - y2 - z2) * src.x + 2.0 * (x * y - z * w) * src.y + 2.0 * (x * z + y * w) * src.z;
    double dy =
        2.0 * (x * y + z * w) * src.x + (w2 - x2 + y2 - z2) * src.y + 2.0 * (y * z - x * w) * src.z;
    double dz =
        2.0 * (x * z - y * w) * src.x + 2.0 * (x * w + y * z) * src.y + (w2 - x2 - y2 + z2) * src.z;
    dst.x = dx;
    dst.y = dy;
    dst.z = dz;
    return dst;
}

void Rotation::ScaleAngle(double factor)
{
    Vector3d axis;
    double angle;
    Get(axis, angle);
    Set(axis, angle * factor);
}

Rotation& Rotation::Invert()
{
    x = -x;
    y = -y;
    z = -z;
    return *this;
}

Rotation Rotation::Inverse() const
{
    return Rotation(-x, -y, -z, w);
}

void Rotation::MakeIdentity()
{
    x = y = z = 0.0;
    w = 1.0;
}

bool Rotation::IsIdentity() const
{
    return ((x == 0.0 && y == 0.0 && z == 0.0) && (w == 1.0 || w == -1.0));
}

bool Rotation::IsEqual(const Rotation& q, double tol) const
{
    // This follows the implementation of Coin3d where the norm
    // (x1-y1)**2 + ... + (x4-y4)**2 is computed.
    // This term can be simplified to
    // 2 - 2*(x1*y1 + ... + x4*y4) so that for the equality we have to check
    // 1 - tol/2 <= x1*y1 + ... + x4*y4
    // This simplification only work if both quats are normalized
    // Is it safe to assume that?
    // Because a quaternion (x1,x2,x3,x4) is equal to (-x1,-x2,-x3,-x4) we use the
    // absolute value of the scalar product
    double dot =
        q.x * x + q.y * y + q.z * z + q.w * w;
    return fabs(dot) >= 1.0 - tol / 2;
}

bool Rotation::IsIdentity(double tol) const
{
    return IsEqual(Rotation(), tol);
}

Matrix4d Rotation::ToMatrix() const
{
    Matrix4d matrix;
    const double l = sqrt(x * x + y * y + z * z + w * w);
    const double nx = x / l;
    const double ny = y / l;
    const double nz = z / l;
    const double nw = w / l;

    matrix[0][0] = 1.0 - 2.0 * (ny * ny + nz * nz);
    matrix[0][1] = 2.0 * (nx * ny + nz * nw);
    matrix[0][2] = 2.0 * (nx * nz - ny * nw);
    matrix[0][3] = 0.0;

    matrix[1][0] = 2.0 * (nx * ny - nz * nw);
    matrix[1][1] = 1.0 - 2.0 * (nx * nx + nz * nz);
    matrix[1][2] = 2.0 * (ny * nz + nx * nw);
    matrix[1][3] = 0.0;

    matrix[2][0] = 2.0 * (nx * nz + ny * nw);
    matrix[2][1] = 2.0 * (ny * nz - nx * nw);
    matrix[2][2] = 1.0 - 2.0 * (nx * nx + ny * ny);
    matrix[2][3] = 0.0;

    matrix[3][0] = 0.0;
    matrix[3][1] = 0.0;
    matrix[3][2] = 0.0;
    matrix[3][3] = 1.0;
    return matrix;
}
